//Debug Tip:
// After Try, adding or remove "use_usbcon", adding or remove "serial begin " in setup, rosserial connection is ok.
// reset roscore & use rosserial_arduino works 
// platformio lib use : rosserial_arduino
// Rootcasue: rosserial_arduino is old lib, use serial port as def, 0.91 rosserial_arduino use tcp as def for ESP32
//so it shows protocol err, change ros.h to use usb as def
// Update ROS.h in lib file, change buffer size to 2048 and use *ArduinoHardware*
// adding publish odom data to ROS
// Todo: enalbe BLE to control speed and angular, publish ble data to ROS for training set 


// run below on Ubuntu for usb serial and Twist cmd ...
//Power on motor before run, or ESP32 will keep reseting...
//roslaunch rosserial_server socket.launch
/***
rostopic pub /cmd_vel geometry_msgs/Twist "linear:
  x: 5 
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.0" -r 10
***/


#include <WiFi.h>

#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Float32.h>
#include <motor.h>
#include <main.h>
// #include <blink_ble.h>
#include <Arduino.h>
#include <Wire.h>
#include <SimpleFOC.h>
// To use the TCP version of rosserial_arduino
// To use the TCP version of rosserial_arduino
//#define ROSSERIAL_ARDUINO_TCP
#include <ros.h>


// WiFi credentials
const char* ssid = "CMCC-Athlon";
const char* password = "haha1234";
// To be update for rosserial wifi, server site need install roserial_server pkg.
IPAddress server(192,168,1,7); // ROS server IP, Raspi here
const uint16_t serverPort = 11411; // Server port




//Wheelbase (meters) - adjust based on your robot,used to cal motor speed
//const float WHEELBASE = 0.2f; 
const float WHEELBASE = 0.2f; 
const float WHEEL_R = 0.03f;

//Register?
nav_msgs::Odometry odom;  // Odom message object, declared globally
ros::Publisher odom_pub("odom", &odom);

unsigned long lastUpdateTime = 0;  // Last update time in microseconds


ros::NodeHandle nh;

//Function to receive Twist cmd from Ubuntu ROS
void cmdVelCallback(const geometry_msgs::Twist& cmd_vel){
    float linear_speed = cmd_vel.linear.x;  // Linear speed in m/s
    float angular_velocity = cmd_vel.angular.z;  // Angular velocity in rad/s
    Serial.print("Twist Callback... Reciver linear.x from Server:  ");
    Serial.println(linear_speed);
    // Implement your logic to drive the vehicle here based on linear_speed and angular_velocity
    float v_left = linear_speed - (angular_velocity * WHEELBASE /2.0);
    float v_right = linear_speed + (angular_velocity*WHEELBASE / 2.0);
    //ROS use m/s, BLDC use rad/S
    v_left = v_left / WHEEL_R;
    v_right = v_right / WHEEL_R;
    set_motor_speed(v_left,v_right);
    
}

ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &cmdVelCallback);


// Function to compute the odom data, publish to Ubuntu ROS
void computeOdom(nav_msgs::Odometry& odom,double leftSpeed, double rightSpeed,double dt) {
    static double x = 0.0, y = 0.0, theta = 0.0;  // Position and orientation
     
    //double L = 0.5;  // Assumed track width, adjust to your robot's specification

    // Calculate the distance traveled by each wheel
   
    double dl = leftSpeed * WHEEL_R * dt;
    double dr = rightSpeed * WHEEL_R * dt;

    // Average distance traveled
    double dCenter = (dl + dr) / 2.0;

    // Change in orientation
    double dTheta = (dr - dl) / WHEELBASE;

    // Update the robot's orientation
    theta += dTheta;

    // Update the robot's position
    x += dCenter * cos(theta);
    y += dCenter * sin(theta);

    // Set the odom message
    odom.pose.pose.position.x = x;// for usb serial, add usb port in cmd line.
// Use the following line if you have a Leonardo or MKR1000
//#define USE_USBCON
    odom.pose.pose.position.y = y;
    // odom.pose.pose.orientation.z = sin(theta / 2);
    // odom.pose.pose.orientation.w = cos(theta / 2);
    // Set the velocity in the odom message
    odom.twist.twist.linear.x = dCenter / dt;  // Linear velocity
    odom.twist.twist.angular.z = dTheta / dt;  // Angular velocity

}


void setup() {
  Serial.begin(115200);
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);

  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    Serial.println("connecting WiFi...");
    delay(500);
    Serial.print(".");
  }
  Serial.println("");
  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
  
   //For V2 Control Board, serial1 test working,swap motor control serial port solve  speed diff issue,why??? 
  //PS: type C接口有方向，有一面无法识别！！！
  //电源线接触问题会导致motor serial 没有输出，可以通过扭矩判断，没有扭矩就是没有上电
  // Motor driver set to 9600 to avoid transfer error
  Serial1.begin(9600, SERIAL_8N1, 13, 15);   // Serial_motor_left, uart1 in schematic，close to corner in layout
  Serial2.begin(9600, SERIAL_8N1, 16, 17); // Serial_motor_right, uart0 in schematic, RX FIRST THEN TX


// Set the connection to rosserial socket server, comment this if use usb connection
  // nh.getHardware()->setConnection(server, serverPort);
  // if (nh.connected()) {
  //   Serial.println("Connected");
  //    } else {
  //   Serial.println("Not Connected");
  // }

// Set the connection to rosserial socket server
  nh.getHardware()->setConnection(server, serverPort);
  nh.initNode();

  // Another way to get IP
  Serial.print("IP = ");
  Serial.println(nh.getHardware()->getLocalIP());

  if (nh.connected()) {
    Serial.println("Connected");
     } else {
    Serial.println("Not Connected");
  }

// Initialize ROS node ;
//  nh.initNode();
  nh.subscribe(sub);
  nh.advertise(odom_pub);
}

void loop() {
  unsigned long currentTime = micros();
  double dt = (currentTime - lastUpdateTime) / 1000000.0;  // Compute dt in seconds

  // double leftSpeed = read_left_motor();  // Get the current speed of the left motor
  // double rightSpeed = read_right_motor();  // Get the current speed of the right motor

  // computeOdom(odom, leftSpeed, rightSpeed, dt);  // Compute the odom data based on the motor speeds

  // odom_pub.publish(&odom);  // Publish the odom data


  // if (nh.connected()) {
  //   Serial.println("Connected");
  //    } else {
  //   Serial.println("Not Connected");
  // }

  nh.spinOnce();
  delay(10);
  lastUpdateTime = currentTime;  // Update lastUpdateTime
}


